#include "Polygon.h"
#include <stdio.h>

#define PI 3.1415926
#define TWOPI 2 * PI

Polygon::Polygon(void)
{
}


Polygon::~Polygon(void)
{
}

void Polygon::buildPolyStructure( initializeFunc setUp )
{
	setUp(&edgeList, &vertList, &edgeCount, &vertCount);

	constructEdgeNormal();
	constructVertNormal();
}

void Polygon::constructEdgeNormal()
{
	for (int i=0; i<(edgeCount); i++)
	{
		int ia = (edgeList)[i].a;
		int ib = (edgeList)[i].b;

		Vert va = (vertList)[ia];
		Vert vb = (vertList)[ib];

		Vec3d es(va.x, va.y, 0);
		Vec3d et(vb.x, vb.y, 0);

		Vec3d el = et - es;
		Vec3d zl(0, 0, 1);

		Vec3d nl = cross(zl, el);
		nl = nl / mag(nl);

		(edgeList)[i].nx = nl[0];
		(edgeList)[i].ny = nl[1];
	}
}

void Polygon::constructVertNormal()
{
	for (int i=0; i<(vertCount); i++)
	{
		int ea = (vertList)[i].ea;
		int eb = (vertList)[i].eb;

		Edge edgeA = (edgeList)[ea];
		Edge edgeB = (edgeList)[eb];

		Vec2d na(edgeA.nx, edgeA.ny);
		Vec2d nb(edgeB.nx, edgeB.ny);

		Vec2d nv = na + nb;
		nv = nv / mag(nv);

		(vertList)[i].nx = nv[0];
		(vertList)[i].ny = nv[1];
	}
}

void Polygon::translatePolygon( Vec2d translate )
{
	for (int i=0; i<vertCount; i++)
	{
		vertList[i].x += translate[0];
		vertList[i].y += translate[1];
	}
}

void Polygon::rotatePolygon( Mat22d rotate )
{
	for (int i=0; i<vertCount; i++)
	{
		Vec2d vv(vertList[i].x, vertList[i].y);

		vv = rotate * vv;

		vertList[i].x = vv[0];
		vertList[i].y = vv[1];
	}

	constructEdgeNormal();

	constructVertNormal();
}

void Polygon::getShortestDistanceFromPointToLine( int index, Vec2d point, double* dist, Vec2d* cpt, Vec2d* cn )
{
	Edge e = edgeList[index];

	Vert pa = vertList[e.a];
	Vert pb = vertList[e.b];

	Vec2d va(pa.x, pa.y);
	Vec2d vb(pb.x, pb.y);
	Vec2d en(e.nx, e.ny);

	// check shortest distance

	Vec2d ab = vb - va;
	Vec2d ap = point - va;
	Vec2d dir = ab / mag(ab);

	double d_b = dot(ab, dir);
	double d_p = dot(ap, dir);

	if (d_p < 0)
	{
		*dist = mag(point - va);
		*cpt = va;
		*cn = Vec2d(pa.nx, pa.ny);
	}else if (abs(d_p) < 1e-8)
	{
		*dist = mag(point - va);
		*cpt = va;
		*cn = Vec2d(pa.nx, pa.ny);
	}else if (d_p > 0 && d_p < d_b)
	{
		Vec2d tt = ap - dir * d_p;
		*dist = mag(tt);
		*cpt = va + dir * d_p;
		*cn = Vec2d(e.nx, e.ny);
	}else if (abs(d_b - d_p) < 1e-8)
	{
		*dist = mag(point - vb);
		*cpt = vb;
		*cn = Vec2d(pb.nx, pb.ny);
	}else if (d_p > d_b)
	{
		*dist = mag(point - vb);
		*cpt = vb;
		*cn = Vec2d(pb.nx, pb.ny);
	}
}

void Polygon::getShortestDistanceFromPointToPoly( Vec2d point, double* dist, Vec2d* cpt, Vec2d* cn )
{
	double curDist, bestDist;
	Vec2d curCpt, curCn, bestCpt, bestCn;

	bestDist = 1e8;	

	for (int i=0; i<edgeCount; i++)
	{
		if (getSideInfo(i, point))
		{
			getShortestDistanceFromPointToLine(i, point, &curDist, &curCpt, &curCn);

			if (bestDist > curDist)
			{
				bestDist = curDist;
				bestCpt = curCpt;
				bestCn = curCn;
			}
		}	
	}

	bool check = getInclusionInfo(point);

	if (!check)
	{
		bestDist = 1e8;
	}else{
		bestDist *= -1;
	}

	*dist = bestDist;
	*cpt = bestCpt;
	*cn = bestCn;
}

bool Polygon::getSideInfo( int index, Vec2d point )
{
	Edge e = edgeList[index];

	Vert pa = vertList[e.a];
	Vert pb = vertList[e.b];

	Vec2d va(pa.x, pa.y);
	Vec2d vb(pb.x, pb.y);
	Vec2d en(e.nx, e.ny);

	Vec2d ab = vb - va;
	Vec2d ap = point - va;
	Vec2d dir = ab / mag(ab);
	
	double d_p = dot(ap, dir);

	Vec2d tt = ap - dir * d_p;

	double check = dot(tt, en);

	if (abs(check) < 1e-8)
	{
		return true;
	}else if (check > 0)
	{
		return false;
	}else if (check < 0)
	{
		return true;
	}
}

bool Polygon::getInclusionInfo( Vec2d point )
{
	double angle = 0;

	for (int i=0; i<edgeCount; i++)
	{
		Edge e = edgeList[i];

		Vert pa = vertList[e.a];
		Vert pb = vertList[e.b];

		Vec2d va(pa.x, pa.y);
		Vec2d vb(pb.x, pb.y);

		Vec2d ap = va - point;
		Vec2d bp = vb - point;

		angle += getAngle(ap[0], ap[1], bp[0], bp[1]);		
	}

	if (abs(angle) < PI)
	{
		return false;
	}else{
		return true;
	}
}

double Polygon::getAngle( double x1, double y1, double x2, double y2 )
{
	double dtheta,theta1,theta2;	

	theta1 = atan2(y1,x1);
	theta2 = atan2(y2,x2);
	dtheta = theta2 - theta1;
	while (dtheta > PI)
		dtheta -= TWOPI;
	while (dtheta < -PI)
		dtheta += TWOPI;

	return(dtheta);
}
